
#include "creature.h"

void Creature::addJoint(opal::Joint *j){
	joints.push_back(j);
}

void Creature::addSolid(opal::Solid *s) {
	solids.push_back(s);
	startStates.push_back(opal::Point3r(0.0,0.0,0.0));
	quaternions.push_back(opal::Quaternion(0.0,0.0,0.0,0.0) );
}

void Creature::applyForce(int solidNum, opal::Vec3r v){
		opal::Solid *s = solids[solidNum];
		opal::Force f;
		f.type = opal::GLOBAL_FORCE;
		f.vec = v;
		f.duration = 0.1;
		//f.pos = opal::Point3r(6.0,6.0,6.0);
		s->addForce(f);
}

void Creature::applyGearedForce(int jointNum, double throttle){

	if(jointNum >= joints.size()){
		return;
	}

	//opal:: GearedMotor* engine;
	opal:: GearedMotor* engine = (opal::GearedMotor* ) motors[jointNum];
//	engine->init(data);
	if(  (engine->getThrottle() + throttle < 1.0)
			&& (engine->getThrottle() + throttle > -1.0)
	) {
		engine->setThrottle(throttle);
	}
/*
	opal:: GearedMotor* engine1 = (opal::GearedMotor* ) motors1[jointNum];
	engine1->init(data);
	if(  (engine1->getThrottle() + throttle < 1.0)
			&& (engine1->getThrottle() + throttle > -1.0)
	) {
		engine1->setThrottle(throttle);
	}
*/
	//motors[jointNum] = engine;

}

void Creature::applyServoForce(int jointNum, double jointAngle){
	/*opal::ServoMotorData data;
	data.joint = joints[jointNum];
	data.mode = opal::DESIRED_ANGLE_MODE;
	data.jointAxisNum = 0;
	double angle =  joints[jointNum]->getAngle(data.jointAxisNum) + jointAngle;
	data.desiredAngle = angle;
	data.maxTorque = 10000.0;
	data.restoreSpeed = 2.0;
	opal::ServoMotor* servo = gSimulator->createServoMotor();
	servo->init(data);
	*/

	if(jointNum >= joints.size()){
			return;
		}

	opal::ServoMotor* servo = (opal::ServoMotor *) servos[jointNum];
	servo->setDesiredAngle(joints[jointNum]->getAngle(0) + jointAngle);

}

void Creature::create(SimSolid *s){
	s->create();
	simSolids.push_back(s);
	addSolid(s->solid);
}

void Creature::readAgent(){

	std::ifstream myfile;

	myfile.open("");//filename.c_str());

	if(myfile.is_open()){
		while (!myfile.eof()) {
			char c;
			myfile>>c;
			while(c!= 'e'){
				
				// joint
				if(c=='j'){
					opal::Joint* joint = gSimulator->createJoint();			
					
				}
			
				// solid
				if(c=='s'){	
					opal::Solid* solid = gSimulator->createSolid();
					double x,y,z,r;
					opal::Matrix44r transform;
					myfile >>x>>y>>z;
					transform.translate(x,y,z);
					myfile >>r>>x>>y>>z;
					transform.rotate(r,x,y,z);
					myfile>>c;
					switch(c){
					case 's':
						myfile>>r;
						createSphere(&solid, transform, r);
						break;
					case 'b':
						myfile>>x>>y>>z;
						createBox(&solid, transform, opal::Vec3r(x,y,z));
						break;
					case 'c':
						myfile>>r>>x;
						createCapsule(&solid, transform, r, x);					
						break;
					} 
					addSolid(solid);
					myfile >>c;	
				}
				
			}
		}
	}
	myfile.close();	
}

void Creature::createAgent(){
	opal::Solid* agent = gSimulator->createSolid();
	opal::Matrix44r transform;
	//applyForceAgent(Vec3r(20.0, 0.0, 0.0));

	this->addSolid(agent);

	int agentType = 3;
	switch(agentType){
		case 0:
			transform.translate(0.0, 4.0, 0.0);
			agent->setTransform(transform);
			//createSphere(&agent);
			break;

		case 1:
			transform.translate(0.0, 4.0, 0.0);
			agent->setTransform(transform);
			//createBox(&agent);
			break;

		case 2: 
			transform.translate(-4.0, 4.0, 0.0);
			transform.rotate(90.0, 0, 1, 0);
			agent->setTransform(transform);
			//createCapsule(&agent);
			break;

		case 3:

			//opal::Solid *solid1 = gSimulator->createSolid();
			opal::Solid *solid2 = gSimulator->createSolid();

			transform.translate(-4.0, 4.0, 0.0);
			transform.rotate(90.0, 0, 1, 0);
			agent->setTransform(transform);

			
			transform.translate(0.0, 0.0, 6.0);
			solid2->setTransform(transform);

			opal::Solid *solid_joint;		

			//createCapsule(&agent);
			//createCapsule(&solid2);
	
			opal::Joint* joint = gSimulator->createJoint();
			createJoint(&joint, agent, solid2);

		/*	opal::ServoMotorData data;
			data.joint = joint;
			data.mode = opal::DESIRED_ANGLE_MODE;
			data.jointAxisNum = 0;
			data.desiredAngle = 90.0;
			data.maxTorque = 10000.0;
			data.restoreSpeed = 2.0;
			opal::ServoMotor* servo = gSimulator->createServoMotor();
			servo->init(data);
*/
			this->addJoint(joint);
			break;
	}

}



void Creature::createSphere(opal::Solid** solid, opal::Matrix44r transform, double radius){
	(*solid)->setTransform(transform);
	opalSamples::Entity* entity = NULL;

	//(*solid)->setLinearDamping(0.2);

	opal::SphereShapeData sphereData;
	sphereData.radius = radius;
	sphereData.offset.translate(0.0, 0.0, 0.0);
	sphereData.material.friction = 1.0;
	(*solid)->addShape(sphereData);
	
	entity = new opalSamples::SphereEntity();
	((opalSamples::SphereEntity*)entity)->setRadius(
				sphereData.radius);

	opal::Vec3r color(randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7));

	entity->setColor(color[0], color[1], color[2]);
	entity->attachToSolid((*solid));
	gEntities->push_back(entity);

}

void Creature::createBox(opal::Solid** solid, opal::Matrix44r transform, opal::Vec3r v){
	(*solid)->setTransform(transform);

	opalSamples::Entity* entity = NULL;

	opal::BoxShapeData boxData;
	boxData.dimensions.set(v.x, v.y, v.z);
	boxData.material.hardness = 0.2;
	boxData.material.bounciness = 0.6;
	boxData.material.friction = 1.0;
	boxData.material.density = 0.4;
	(*solid)->addShape(boxData);

	entity = new opalSamples::BoxEntity();
			((opalSamples::BoxEntity*)entity)->setDimensions(
				boxData.dimensions);

	opal::Vec3r color(randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7));

	entity->setColor(color[0], color[1], color[2]);
	entity->attachToSolid((*solid));
	gEntities->push_back(entity);
}

void Creature::createCapsule(opal::Solid** solid, opal::Matrix44r transform, double radius, double length) {
	(*solid)->setTransform(transform);
	opalSamples::Entity* entity = NULL;

	opal::CapsuleShapeData capsuleShape;	
	capsuleShape.radius = 1.0;
	capsuleShape.length = 4;
	(* solid)->addShape(capsuleShape);


	entity = new opalSamples::CapsuleEntity();
	((opalSamples::CapsuleEntity*)entity)->setDimensions(
		capsuleShape.radius, capsuleShape.length);

	opal::Vec3r color(randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7));
	entity->setColor(color[0], color[1], color[2]);
	entity->attachToSolid((*solid));

	gEntities->push_back(entity);
}

void Creature::joinSolids(int solid0, int solid1){
	opal::Joint* joint = gSimulator->createJoint();

	opal::JointData jointData;
	jointData.setType(opal::HINGE_JOINT);
//	jointData.setType(opal::BALL_JOINT);
	jointData.solid0 = solids[solid0] ;
	jointData.solid1 = solids[solid1] ;

	opal::Point3r p1 = solids[solid0]->getPosition();
	opal::Point3r p2 = solids[solid1]->getPosition();
	//(p2 - p1).normalize();
	//opal::Matrix44r transform;
	//transform.rotate()
	//solids[solid1]->setTransform(transform)
	opal::Point3r jointPos = opal::Point3r((p1.x + p2.x)/2.0, (p2.y + p1.y)/2.0, (p1.z + p2.z)/2.0);

	jointData.anchor =	opal::Point3r(jointPos);
	jointData.axis[0].direction = opal::Vec3r(0.0, 0.0, 1.0);

	joint->init(jointData);
	joints.push_back(joint);

	opal::GearedMotorData data;
	data.joint = joints[joints.size()-1];
	data.jointAxisNum = 0;
	data.maxTorque = 100.0;
	data.maxVelocity = 2000.0;

	opal:: GearedMotor* engine = gSimulator->createGearedMotor();
	engine->init(data);
	motors.push_back(engine);


	opal::ServoMotorData servodata;
	servodata.joint = joints[0];
	servodata.mode = opal::DESIRED_ANGLE_MODE;
	servodata.jointAxisNum = 0;
	double angle =  joints[0]->getAngle(servodata.jointAxisNum);// + jointAngle;
	servodata.desiredAngle = angle;
	servodata.maxTorque = 10000.0;
	servodata.restoreSpeed = 2.0;
	opal::ServoMotor* servo = gSimulator->createServoMotor();
	servo->init(servodata);


	servos.push_back(servo);
/*
	opal:: GearedMotor* engine1 = gSimulator->createGearedMotor();
	motors1.push_back(engine1);
*/
}

void Creature::breakJoints(int jointNum){

	if( jointNum < joints.size() && jointNum >= 0) {
		gSimulator->destroyJoint(joints[jointNum]);
		joints.erase(joints.begin() + jointNum);
		motors.erase(motors.begin() + jointNum);
		servos.erase(servos.begin() + jointNum);
	}
}

void Creature::createJoint(opal::Joint **joint, opal::Solid *solid0, opal::Solid *solid1) {	

	opal::JointData jointData;
	jointData.setType(opal::HINGE_JOINT);
//	jointData.setType(opal::BALL_JOINT);
	jointData.solid0 = solid0;
	jointData.solid1 = solid1;

	opal::Point3r p1 = solid1->getPosition();
	jointData.anchor =	opal::Point3r(p1.x + 2.0, p1.y , p1.z);
	jointData.axis[0].direction = opal::Vec3r(0.0, 1.0, 0.0);

	(*joint)->init(jointData);


	// just to show the anchor
opalSamples::Entity* entity = NULL;
	opal::Solid *solid = gSimulator->createSolid();
	opal::Matrix44r transform;
	opal::Point3r p = (*joint)->getAnchor();
	std::cout<<"Anchor: "<<p.x<<" "<<p.y<<" "<<p.z<<"\n";
	transform.translate(p.x + 2.0, p.y, p.z);
	solid->setTransform(transform);

	opal::CapsuleShapeData capsuleShape;	
	
	capsuleShape.radius = 0.2;
	capsuleShape.length = 1;
	solid->addShape(capsuleShape);
	entity = new opalSamples::CapsuleEntity();
	((opalSamples::CapsuleEntity*)entity)->setDimensions(
		capsuleShape.radius, capsuleShape.length);
opal::Vec3r color(randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7), 
		randomRealUniform(0, (opal::real)0.7));
	entity->setColor(color[0], color[1], color[2]);
	entity->attachToSolid(solid);
	gEntities->push_back(entity);

}

opal::Point3r Creature::getAgentPosition(int numSolid){
	return solids[numSolid]->getPosition();
}

void Creature::setAgentPosition(int numSolid, opal::Point3r p){
	//std::cout<<" --> set agent pos"<<(solids[0])->getPosition()<<std::endl;
	solids[numSolid]->setPosition(p.x,p.y-1.0,p.z);
}

void Creature::resetPositions(opal::Point3r minWorld, opal::Point3r maxWorld){
	opal::Point3r random_position;
	bool randomStart = false;


	for(int i= this->getNumJoints() -1 ; i>=0 ; i--){
		breakJoints(i);
	}

	for(int i=0; i< this->getNumSolids(); i++){
		//std::cout<<"before creature::resetPositions()" <<(solids[i])->getPosition()<<std::endl;
		if(randomStart && !solids[i]->isStatic()) {
			startStates[i].x = (rand() % (int)(maxWorld.x-2 - (minWorld.x+2))) - maxWorld.x +2;
			//startStates[i].y = startStates[i].y;
			startStates[i].z = (rand() % (int)(maxWorld.z-2 - (minWorld.z+2))) - maxWorld.z +2;
			solids[i]->setPosition(startStates[i]);
			//std::cout << random_position.x << "  "<< random_position.y << "  "<<random_position.z <<std::endl;
		} else {
			solids[i]->setPosition(startStates[i]);
		}
		//std::cout<<"Start states " << startStates[i].x << "  " << startStates[i].y << "  "<< startStates[i].z;
		//std::cout<<"after creature::resetPositions()" <<(solids[i])->getPosition()<<std::endl;

		solids[i]->setLocalLinearVel(opal::Vec3r(0.0,0.0,0.0));
		solids[i]->setLocalAngularVel(opal::Vec3r(0.0,0.0,0.0));

		solids[i]->setQuaternion(quaternions[i]);
	}



}

opal::Point3r Creature::getStartState(int numSolid){
	return startStates[numSolid];
}

void Creature::removeForces(){
	// remove
}

opal::real Creature::randomRealUniform(opal::real min, opal::real max)
{
	return (opal::real)rand() / RAND_MAX * (max - min) + min;
}

void Creature::setOneStartState (int numSolid, opal::Point3r sstate){
	startStates[numSolid] = sstate;
}

void Creature::setStartStates(){
	for(int i=0; i< this->getNumSolids(); i++){
			startStates[i]  = getAgentPosition(i);
			quaternions[i] = solids[i]->getQuaternion();
		//	std::cout<<"creature::setStartStates()"<<startStates[i]<<std::endl;
	}
}

